#include <mc_ros/visualization/controller_visualization.h>

MCVisualization::MCVisualization(){
    path_global_pub_ = nh_.advertise<nav_msgs::Path>("/mc_global_path", 1, true);
    path_local_pub_ = nh_.advertise<nav_msgs::Path>("/mc_local_path", 1, true);
    path_tracked_Pub_ = nh_.advertise<nav_msgs::Path>("/mc_tracked_path", 1);
    robot_model_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/robot_model", 1);
    path_global_marker_pub_ = nh_.advertise<visualization_msgs::MarkerArray>("/mc_global_path_marker", false);
    lookahead_pose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/lookahead_pose", 10);
}

// 设置机器人模型
void MCVisualization::setRobotModel(boost::shared_ptr<ModelBase> model_ptr){
    robotModelPtr_ = model_ptr;
}

// 发布全局路径
void MCVisualization::publishGolbalPath(vector<geometry_msgs::PoseStamped> poses){
    nav_msgs::Path path_msg;
    path_msg.header.stamp = ros::Time::now();
    path_msg.header.frame_id = "map";
    path_msg.poses = poses;
    path_global_pub_.publish(path_msg); 

    // 发布全局路径Marker 
    publishGolbalPathMarker(poses);
}

void MCVisualization::publishGolbalPath(vector<geometry_msgs::Pose> poses){
    vector<geometry_msgs::PoseStamped> poses_stamp;
    for(auto pose : poses){
        poses_stamp.emplace_back(msg_convert::toPoseStamped(pose, "map"));
    }
    publishGolbalPath(poses_stamp);
}


// 发布局部路径
void MCVisualization::publishLocalPath(vector<geometry_msgs::PoseStamped> poses){
    nav_msgs::Path path_msg;
    path_msg.header.stamp = ros::Time::now();
    path_msg.header.frame_id = "base_link";
    path_msg.poses = poses;
    path_local_pub_.publish(path_msg); 
}

void MCVisualization::publishLocalPath(vector<geometry_msgs::Pose> poses){
    vector<geometry_msgs::PoseStamped> poses_stamp;
    for(auto pose : poses){
        poses_stamp.emplace_back(msg_convert::toPoseStamped(pose, "base_link"));
    }
    publishLocalPath(poses_stamp);
}


// 发布跟踪轨迹
void MCVisualization::publishTrackingPose(geometry_msgs::PoseStamped robot_pose){
    trackedPath_.addPoseStamped(robot_pose);
    path_tracked_Pub_.publish(trackedPath_.toPath("map"));
}

// 发布机器人模型
void MCVisualization::publishRobotModel(string frame_id){
    if(!robotModelPtr_){
        LOG(WARNING) << "robotModelPtr_ is nullptr";
        return;
    }
    robot_model_marker_pub_.publish(robotModelPtr_->getRobotModelMarkerArray(frame_id));
}

// 发布全局路径Marker 路径需要有方向信息提供
void MCVisualization::publishGolbalPathMarker(vector<geometry_msgs::PoseStamped> poses){
    visualization_msgs::MarkerArray markerArray_msg;

    // 计算距离 避免开平方
    auto distance_pow = [](geometry_msgs::Pose p1, geometry_msgs::Pose p2) -> double {
        return pow(p1.position.x - p2.position.x, 2) + pow(p1.position.y - p2.position.y, 2);
    };

    // 点集坐标变换
    auto transformedPoints = [](vector<geometry_msgs::Point> points, geometry_msgs::Pose p) -> vector<geometry_msgs::Point> {
        vector<geometry_msgs::Point> transformed_points;
        double yaw = tf2::getYaw(p.orientation);
        for(auto point : points){
            geometry_msgs::Point transformed_point;
            transformed_point.x = point.x * cos(yaw) - point.y * sin(yaw) + p.position.x;
            transformed_point.y = point.x * sin(yaw) + point.y * cos(yaw) + p.position.y;
            transformed_point.z = point.z;
            transformed_points.emplace_back(transformed_point);
        }
        return transformed_points;
    };

    RobotModel robotModel = robotModelPtr_->getRobotModel();
    double robot_radius = robotModel.robot_radius * 1.2;
    double dis_limit = robotModel.robot_radius * robotModel.robot_radius;
    vector<geometry_msgs::Point> robot_footprints = robotModel.robot_footprints;
    if(robot_footprints.empty()){
        LOG(WARNING) << "robot_footprints is empty";
        return;
    }
    robot_footprints.emplace_back(robot_footprints.front());    // 闭合多边形

    
    // 提取关键点
    vector<geometry_msgs::Pose> key_points;
    key_points.emplace_back(poses.front().pose);
    for(auto pose_stamp : poses){
        if(distance_pow(key_points.back(), pose_stamp.pose) > dis_limit){
            key_points.emplace_back(pose_stamp.pose);
        }
    }
    key_points.emplace_back(poses.back().pose);

    if(!last_global_path_marker_.markers.empty()){
        // 删除旧数据
        for(size_t i = 0; i < last_global_path_marker_.markers.size(); i++){
            last_global_path_marker_.markers[i].action = visualization_msgs::Marker::DELETE;         
        }
        path_global_marker_pub_.publish(last_global_path_marker_); 
    }

    // 插入Marker
    int id = 0;
    for(auto pose : key_points){
        // 插入点 球体Marker
        visualization_msgs::Marker marker_msg = rvizMarker_.genSphereMarker(pose, 0.01);
        rvizMarker_.setMarkerHeader(marker_msg, "map", "key_points");
        rvizMarker_.setMarkerId(marker_msg, id);
        rvizMarker_.setMarkerColor(marker_msg, rvizMarker_.colorBlue());
        markerArray_msg.markers.push_back(marker_msg);
        id++;
        // 插入连续线段
        vector<geometry_msgs::Point> transformed_footprints = transformedPoints(robot_footprints, pose);
        visualization_msgs::Marker footprints_marker_msg = rvizMarker_.genLineStripMarker(transformed_footprints);
        rvizMarker_.setMarkerHeader(footprints_marker_msg, "map", "robot_footprints");
        rvizMarker_.setMarkerId(footprints_marker_msg, id);
        rvizMarker_.setMarkerColor(footprints_marker_msg, rvizMarker_.colorBlue(0.3));
        rvizMarker_.setMarkerScale(footprints_marker_msg, rvizMarker_.genScale(0.01, 0.01, 0.01));
        markerArray_msg.markers.push_back(footprints_marker_msg);
        id++;
    }

    last_global_path_marker_ = markerArray_msg;
    path_global_marker_pub_.publish(markerArray_msg);
}


// 发布预瞄点
void MCVisualization::publishLookaheadPose(geometry_msgs::PoseStamped lookahead_pose, string frame_id){
    lookahead_pose.header.stamp = ros::Time::now();
    lookahead_pose.header.frame_id = frame_id;
    lookahead_pose_pub_.publish(lookahead_pose);
}